import math
import matplotlib.pyplot as plt
import utm
import json
listo=[]
listoo=[]
listr=[]
lista2= []
listalt = []
hn = 0
st = ''
with open('mission.json','r') as f:
    st = f.read()
#print(st)
l = []
js = json.loads(st)



so = js['stationaryObstacles']

for o in so:
    listr.append(o['radius'])
    listo.append(o['latitude'])
    listoo.append(o['longitude'])
    lista2.append(o['height'])

print(lista2)    


#print(listr)
col_num = 2
col_data = []
delimiter = "\t"
liststart1=[]
liststart2=[]
#listo=[]
#listoo=[]
#listr=[]
finallistlat=[]

finallistlong=[]

finalh  =[]

t0=[]
t1=[]
t2=[]
t3=[]
t4=[]
t5=[]
t6=[]
t7=[]
t10=[]
t11=[]
t12=[]

wp = js['waypoints']
for o in wp:
    liststart1.append(o['latitude'])
    liststart2.append(o['longitude'])
    listalt.append(o['altitude'])

finallistlat.append(round(liststart1[0],6))
finallistlong.append(round(liststart2[0],6))
finalh.append(listalt[0])
print(listalt)


i=-1



       


r= 300

for i in range(0, len(liststart1)-1):
    lat = liststart1[i]
    lat2 = liststart1[i+1]
    lon = liststart2[i]
    lon2 = liststart2[i+1]
    for j in range(0, len(listo)):
        lat3  = listo[j]
        lon3 = listoo[j]
        r = listr[j]
        
        x11=utm.from_latlon(lat,lon)
        x22=utm.from_latlon(lat2,lon2)
        z=x11[2]    
        ze=x11[3]
        o11=utm.from_latlon(lat3, lon3)  
        x1=x11[0]
        x2=x22[0]
        y1=x11[1]
        y2=x22[1]
        o1=o11[0]
        o2=o11[1]
        radius=r
        r=r*0.3
        tttt=r
        a = math.sqrt((o1-x1)*(o1-x1)+(o2-y1)*(o2-y1))
        b = math.sqrt((o1-x2)*(o1-x2)+(o2-y2)*(o2-y2))
        k = (math.sqrt((y2-y1)*(y2-y1)+(x2-x1)*(x2-x1)))
        
        t = (a*a-k*k-b*b)/(-2*k)
        q =abs(b*b- math.pow(t,2))
        p = math.sqrt(q); 
        
        if(p<=r):
           ss=1
        else:
             ss=0
             print("Line does not pass")
             lat = liststart1[i]
             lat2 = liststart1[i+1]
             lon = liststart2[i]
             lon2 = liststart2[i+1]
             continue
        if((min(x1,x2,o1)==o1 and min(y1,y2,o2)==o2) or ((max(x1,x2,o1)==o1) and max(y1,y2,o2)==o2) or ((max(x1,x2,o1)==o1) and min(y1,y2,o2)==o2) or ((min(x1,x2,o1)==o1) and max(y1,y2,o2)==o2)):
           print("LINE DOES NOT PASS")
           lat = liststart1[i]
           lat2 = liststart1[i+1]
           lon = liststart2[i]
           lon2 = liststart2[i+1]
           ss=0
           continue
           
        if(ss==1):
           if((x2-x1)==0):
             t=0
           else:    
               t=(y2-y1)/(x2-x1)
           if(t<0):
              q=math.atan2(t,1)-3.14/2
           elif(t>0):
                q=math.atan2(t,1)+3.14/2
           else:
               q=0
           trt = math.atan2((y1-o2),(x1-o1))
           trt2 = math.atan2((y2-o2),(x2-o1))
           r=r*1.414
           m=r
           trtx = r*math.cos(trt)
           trty = r*math.sin(trt)
           trttrx= r*math.cos(trt2)
           trttry = r*math.sin(trt2)
           a0= m*(math.cos(q))+o1
           a= m*(math.sin(q))+o2
           t0 = o1-m*(math.cos(q))
           ttt = o2-m*(math.sin(q))
           trtx=trtx+o1
           trty=trty+o2
           trttrx=trttrx+o1
           trttry=trttry+o2
           if((x1-a0)*(x1-a0)<=(x1-t0)*(x1-t0)):
              x=a0
              y=a
              hn = listalt[i]
      
           else:
               x=t0#avoideed pt
               y=ttt#avoideed pt
               d1 = math.sqrt((x2-x)**2+(y2-y)**2)
               d2 = math.sqrt((x1-x)**2+(y1-y)**2)
               dh = listalt[i+1]-listalt[i]
               hn = listalt[i]
              
               if (dh!=0):
                  '''
                  print('#################################')
                  hn = float((d2/(d1+d2))*(dh))
                  hn=listalt[i]
                  hn = "{:.2f}".format(hn)
                  '''
               else:
                 ''' hn = listalt[i]'''
                  
           d1 = math.sqrt((x2-x)**2+(y2-y)**2)
           d2 = math.sqrt((x1-x)**2+(y1-y)**2)
           dh = listalt[i+1]-listalt[i]
           if (dh!=0):
                    
                    print('#################################')
                    hn = float((d2/(d1+d2))*(dh))
                    hn= listalt[i]-hn*(dh/abs(dh))
                    hn = "{:.2f}".format(hn)
                  
           else:
               hn = listalt[i]
           fig=plt.figure()
           ax=fig.add_subplot(1,1,1)
           plt.plot(trtx,trty, 'ro')
           plt.plot(trttrx, trttry , 'ro')
           plt.plot([trtx,x], [trty,y], color="red")
           plt.plot([trttrx,x], [trttry,y], color="red")
           plt.plot([x1,trtx],[y1,trty], color= "red")
           plt.plot([x2,trttrx],[y2,trttry], color= "red")
           plt.plot([x1,x],[y1,y], color="black")
           plt.plot([x2,x],[y2,y], color="black")
           centreCircle = plt.Circle((o1,o2),tttt,color="red",fill=False)
           ax.add_patch(centreCircle)
           plt.show()
           
           
           print("x1")
           print(x1)
           print("x2")
           print(x2)
           print("y1")
           print(y1)
           print("y2")
           print(y2)
           print("o1")
           print(o1)
           print("o2")
           print(o2)
           print(".........")
           uk = utm.to_latlon(x,y,z,ze)
           latfinal = uk[0]
           lonfinal = uk[1]
           uk2=utm.to_latlon(trtx,trty,z,ze)
           latfinalpre= uk2[0]
           lonfinalpre = uk2[1]
           uk3=utm.to_latlon(trttrx, trttry, z,ze)
           latfinalpost= uk3[0]
           lonfinalpost = uk3[1]
           
           
           finallistlong.append(round((lonfinalpre),6))
           finallistlong.append(round( ( lonfinal),6))
           finallistlong.append(round((lonfinalpost),6))
           finallistlat.append(round((latfinalpre),6))
           finallistlat.append(round((latfinal),6))
           finallistlat.append(round((latfinalpost),6))
           finalh.append(abs(float(hn)))
           finalh.append(abs(float(hn)))
           finalh.append(abs(float(hn)))
           lat = liststart1[i]
           lat2 = liststart1[i+1]
           lon = liststart2[i]
           lon2 = liststart2[i+1]
    
    print("long:")
    print(lon2)
    print("lat:")
    print(lat2)
    
    finallistlong.append ( round( lon2,6))
    
    finallistlat.append ( round (lat2,6))
    finalh.append(listalt[i+1])
    print(finallistlong)
    print(finallistlat)
    
    
    
finallistlat. append (liststart1[len(liststart1)-1])
finallistlong.  append (liststart2[len(liststart2)-1])
finalh.append(listalt[len(listalt)-1])


print(finalh)
l0 = []
l1 = []
l2 = []
l3 = []
l4 = []
l5 = []
l6 = []
l7 = []

for i in range(len(finalh)-1):
    l1.append(0)
    l2.append(3)
    l0.append(i)
    l3.append(16)
    l4.append(0.00000000)
    l5.append(0.00000000)
    l6.append(0.00000000)
    l7.append(0.00000000)



    
l1[0] =1
l2[0] = 0
l3[1] = 22
l3[len(l3)-1] = 21
print(l0)    

col_format = "{:<15}" *11  + "\n"   # 2 left-justfied columns with 5 character width
#with open("waypoint4.mission",'w+') as of:
    

with open("sid.mission", 'w+') as of:
    of.write('QGC WPL 110 \n')
    
   # for x in zip(t0,t1,t2,t3,t4,t5,t6,t7,finallistlat, finallistlong,t10,t11):
       # of.write(col_format.format(*x))
    for x in zip(l0,l1,l2,l3,l4,l5,l6,l7,finallistlat, finallistlong,finalh):
        of.write(col_format.format(*x))

        
print(len(finallistlat))
#f = open('waypoints4.waypoints')

